#include <stdio.h>      // standard input / output functions
#include <stdlib.h>
#include <string.h>     // string function definitions
#include <unistd.h>     // UNIX standard function definitions
#include <fcntl.h>      // File control definitions
#include <errno.h>      // Error number definitions
#include <iostream>

#include "SerCom.h"

using namespace std;

SerCom *SerCom::s_instance = 0;

SerCom *SerCom::get()
{
    if( ! s_instance ) {
        s_instance = new SerCom();
    }
    return s_instance;
}


bool SerCom::init( const char *ttystr )
{
    m_usb = open( ttystr, O_RDWR| O_NOCTTY );
    struct termios tty;
    memset (&tty, 0, sizeof tty);

    /* Error Handling */
    if ( tcgetattr ( m_usb, &tty ) != 0 )
    {
        cout << "Error "
             << errno
             << " from tcgetattr: "
             << strerror(errno) << endl;
        return false;
    }

    /* Save old tty parameters */
    m_oldTty = tty;

    /* Set Baud Rate */
    cfsetospeed ( &tty, ( speed_t ) B19200 );
    cfsetispeed ( &tty, ( speed_t ) B19200 );

    /* Setting other Port Stuff */
    tty.c_cflag     &=  ~PARENB;           // Make 8n1
    tty.c_cflag     &=  ~CSTOPB;
    tty.c_cflag     &=  ~CSIZE;
    tty.c_cflag     |=  CS8;

    tty.c_cflag     &=  ~CRTSCTS;           // no flow control
    tty.c_cc[VMIN]  =   1;                  // read doesn't block
    tty.c_cc[VTIME] =   10;                  // 0.5 seconds read timeout
    tty.c_cflag     |=  CREAD | CLOCAL;     // turn on READ & ignore ctrl lines


    /* Make raw */
    cfmakeraw(&tty);

    /* Flush Port, then applies attributes */
    tcflush( m_usb, TCIFLUSH );
    if ( tcsetattr ( m_usb, TCSANOW, &tty ) != 0) {
        cout << "Error " << errno << " from tcsetattr" << endl;
        return false;
    }
    fcntl( m_usb, F_SETFL, O_NONBLOCK );
    return true;
}


void SerCom::write( const char *cmd, int len )
{
    int n_written = 0;
    do {
        n_written += ::write( m_usb, &cmd[ n_written ], 1 );
    }
    while ( n_written < len
            && cmd[ n_written - 1 ] != '\r'
            && cmd[ n_written - 1 ] != '\n'
            && n_written > 0 );
}


void SerCom::write( std::string &msg )
{
    this->write( msg.c_str(), msg.size() );
}


std::string SerCom::read()
{
    int n = 0;
    char buf;
    std::string response;
    int numTries = 1000;
    do {
        n = ::read( m_usb, &buf, 1 );
        response.append( &buf, 1 );
        usleep( 5 );
    } while( numTries-- && buf != '\r' );

    if (numTries == 0) {
    //    cout << "Error reading: " << std::endl;// << strerror(errno) << endl;
	return "Error";
    }
    else if (n == 0) {
        cout << "Read nothing!" << endl;
    }
    return response;
}


void SerCom::close()
{
    /* Flush Port, then applies attributes */
//    tcflush( m_usb, TCIFLUSH );
//    if ( tcsetattr ( m_usb, TCSANOW, &m_oldTty) != 0) {
//        cout << "Error " << errno << " from tcsetattr while restting" << endl;
//    }
//    std::cout << "Closing device" << std::endl;
//    fcntl( m_usb, F_SETFL, ~O_NONBLOCK );
//    ::close( m_usb );
}
